package kinect_biomec.Logic;


import Testes.SimpleOpenNI;
import processing.core.PVector;

public class CalculoCentroDeGravidade {

	private SimpleOpenNI context;

	public static final int esquerdo = 0;
	public static final int direito = 1;


        CalculoCentroDeGravidade(Testes.SimpleOpenNI context) {
            this.context = context;
        }

	// /////TRONCO/////////////////////
	public PVector getEixoDoQuadril(int userId){
		PVector quadrilEsquerdo = new PVector();
		PVector quadrilDireito = new PVector();
		PVector eixoQuadril = new PVector();
		
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HIP,
				quadrilEsquerdo);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HIP,
				quadrilDireito);

		quadrilDireito = converterPara2D(quadrilDireito);
		quadrilEsquerdo = converterPara2D(quadrilEsquerdo);

		eixoQuadril.x = (quadrilDireito.x + quadrilEsquerdo.x) / 2;
		eixoQuadril.y = (quadrilDireito.y + quadrilEsquerdo.y) / 2;
		
		return eixoQuadril;
	}
	
	public PVector getCentroGravidadeDoTronco(int userId) {
		PVector neck = new PVector();

		PVector eixoQuadril = new PVector();

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_NECK, neck);

		neck = converterPara2D(neck);

		eixoQuadril=getEixoDoQuadril(userId);

		// line(neck.x, neck.y, eixoQuadril.x, eixoQuadril.y);

		return calcCentroGravidadeDoTronco(eixoQuadril, neck);
	}

	private PVector calcCentroGravidadeDoTronco(PVector eixoQuadril,
			PVector neck) {
		eixoQuadril.mult(0.62f);
		neck.mult(0.38f);

		eixoQuadril.add(neck);
		return eixoQuadril;
	}

	private void adicionarMomentoDoTronco(int userId) {

		PVector momentoGravidadeDotronco = getCentroGravidadeDoTronco(userId);

		somaMomentosX += (momentoGravidadeDotronco.x) * 0.507f;
		somaMomentosY += (momentoGravidadeDotronco.y) * 0.507f;
	}

	// //////////////////////////////////////////////////////////
	// /////////////// Parte superior do Braço //////////////////

	private PVector calcCentroGravidadeDaParteSuperiorDoBraco(PVector shoulder,
			PVector elbow) {

		shoulder.mult(0.487f);
		elbow.mult(0.513f);
		shoulder.add(elbow);

		return shoulder;
	}

	public PVector[] getCentroGravidadeDaParteSuperiorDosBracos(int userId) {
		PVector ombroEsquerdo = new PVector();
		PVector ombroDireito = new PVector();

		PVector cotoveloEsquerdo = new PVector();
		PVector cotoveloDireito = new PVector();

		context.getJointPositionSkeleton(userId,
				SimpleOpenNI.SKEL_LEFT_SHOULDER, ombroEsquerdo);
		context.getJointPositionSkeleton(userId,
				SimpleOpenNI.SKEL_RIGHT_SHOULDER, ombroDireito);

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW,
				cotoveloEsquerdo);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW,
				cotoveloDireito);

		cotoveloDireito = converterPara2D(cotoveloDireito);
		cotoveloEsquerdo = converterPara2D(cotoveloEsquerdo);

		ombroEsquerdo = converterPara2D(ombroEsquerdo);
		ombroDireito = converterPara2D(ombroDireito);

		PVector centroDeGravidadeDaParteSuperiorDoBracoDireito = calcCentroGravidadeDaParteSuperiorDoBraco(
				ombroDireito, cotoveloDireito);

		PVector centroDeGravidadeDaParteSuperiorDoBracoEsquerdo = calcCentroGravidadeDaParteSuperiorDoBraco(
				ombroEsquerdo, cotoveloEsquerdo);

		PVector[] centroGravidadeDaParteSuperiorDosBracos = {
				centroDeGravidadeDaParteSuperiorDoBracoEsquerdo,
				centroDeGravidadeDaParteSuperiorDoBracoDireito };

		return centroGravidadeDaParteSuperiorDosBracos;
	}

	private void adicionarMomentoBracoSuperior(int userId) {
		PVector[] centroGravidadeDaParteSuperiorDoBraco = getCentroGravidadeDaParteSuperiorDosBracos(userId);

		somaMomentosX += (centroGravidadeDaParteSuperiorDoBraco[esquerdo].x) * 0.026;
		somaMomentosY += (centroGravidadeDaParteSuperiorDoBraco[esquerdo].y) * 0.026;
		somaMomentosX += (centroGravidadeDaParteSuperiorDoBraco[direito].x) * 0.026;
		somaMomentosY += (centroGravidadeDaParteSuperiorDoBraco[direito].y) * 0.026;
	}

	// /////////////////////////////////////////////////////////////////////////
	// ///////////////////// Coxas /////////////////////////////////////////////

	private PVector calcCentroGravidadeDaCoxa(PVector hip, PVector knee) {

		hip.mult(0.628f);
		knee.mult(0.372f);
		hip.add(knee);

		return hip;
	}

	public PVector[] getCentroGravidadeDasCoxas(int userId) {
		PVector quadrilEsquerdo = new PVector();
		PVector quadrilDireito = new PVector();

		PVector joelhoEsquerdo = new PVector();
		PVector joelhoDireito = new PVector();

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HIP,
				quadrilEsquerdo);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HIP,
				quadrilDireito);

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_KNEE,
				joelhoEsquerdo);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_KNEE,
				joelhoDireito);

		quadrilDireito = converterPara2D(quadrilDireito);
		quadrilEsquerdo = converterPara2D(quadrilEsquerdo);

		joelhoEsquerdo = converterPara2D(joelhoEsquerdo);
		joelhoDireito = converterPara2D(joelhoDireito);

		PVector centroDeGravidadeDaCoxaDireita = calcCentroGravidadeDaCoxa(
				quadrilDireito, joelhoDireito);
		PVector centroDeGravidadeDaCoxaEsquerda = calcCentroGravidadeDaCoxa(
				quadrilEsquerdo, joelhoEsquerdo);

		PVector[] centroGravidadeDasCoxas = { centroDeGravidadeDaCoxaEsquerda,
				centroDeGravidadeDaCoxaDireita };

		return centroGravidadeDasCoxas;
	}

	private void adicionarMomentoDasCoxas(int userId) {
		PVector[] centroGravidadeDaCoxa = getCentroGravidadeDasCoxas(userId);

		somaMomentosX += centroGravidadeDaCoxa[esquerdo].x * 0.103;
		somaMomentosY += (centroGravidadeDaCoxa[esquerdo].y) * 0.103;
		somaMomentosX += centroGravidadeDaCoxa[direito].x * 0.103;
		somaMomentosY += (centroGravidadeDaCoxa[direito].y) * 0.103;
	}

	// /////////////////////////////////////////////////////////////////////////
	// ///////////////////// Cabeça ////////////////////////////////////////////
	private PVector getCentroGravidadeDaCabeca(int userId) {
		PVector centroDeGravidadeDaCabeca = new PVector();

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD,
				centroDeGravidadeDaCabeca);

		return converterPara2D(centroDeGravidadeDaCabeca);
	}

	private void adicionarMomentoDaCabeca(int userId) {
		PVector centroDeGravidadeDaCabeca = getCentroGravidadeDaCabeca(userId);
		somaMomentosX += centroDeGravidadeDaCabeca.x * 0.073;
		somaMomentosY += centroDeGravidadeDaCabeca.y * 0.073;
	}

	// ////////////////////////////////////////////////////////////////////////
	// //////////////////////// Panturrilha ///////////////////////////////////
	private PVector calcCentroGravidadeDaPanturrilha(PVector knee, PVector foot) {

		knee.mult(0.60f);
		foot.mult(0.40f);
		foot.add(knee);

		return foot;
	}

	public PVector[] getCentroGravidadeDaPanturrilha(int userId) {

		PVector joelhoEsquerdo = new PVector();
		PVector joelhoDireito = new PVector();

		PVector peEsquerdo = new PVector();
		PVector peDireito = new PVector();

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_KNEE,
				joelhoEsquerdo);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_KNEE,
				joelhoDireito);

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_FOOT,
				peEsquerdo);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_FOOT,
				peDireito);

		joelhoEsquerdo = converterPara2D(joelhoEsquerdo);
		joelhoDireito = converterPara2D(joelhoDireito);

		peEsquerdo = converterPara2D(peEsquerdo);
		peDireito = converterPara2D(peDireito);

		PVector centroDeGravidadeDaPanturrilhaEsquerda = calcCentroGravidadeDaPanturrilha(
				joelhoEsquerdo, peEsquerdo);

		PVector centroDeGravidadeDaPanturrilhaDireita = calcCentroGravidadeDaPanturrilha(
				joelhoDireito, peDireito);

		PVector[] centrosDeGravidadeDasPanturrilhas = {
				centroDeGravidadeDaPanturrilhaEsquerda,
				centroDeGravidadeDaPanturrilhaDireita };
		return centrosDeGravidadeDasPanturrilhas;
	}

	private void adicionarMomentoDaPanturrilha(int userId) {
		PVector[] centroGravidadeDaPanturrilha = getCentroGravidadeDaPanturrilha(userId);

		somaMomentosX += centroGravidadeDaPanturrilha[esquerdo].x * 0.043;
		somaMomentosY += (centroGravidadeDaPanturrilha[esquerdo].y) * 0.043;
		somaMomentosX += centroGravidadeDaPanturrilha[direito].x * 0.043;
		somaMomentosY += (centroGravidadeDaPanturrilha[direito].y) * 0.043;
	}

	// ////////////////////////////////////////////////////////////////////////
	// ///////////////////////////Mão//////////////////////////////////////////
	private PVector[] getCentroGravidadeDasMaos(int userId) {

		PVector centroDeGravidadeDaMaoEsquerda = new PVector();
		PVector centroDeGravidadeDaMaoDireita = new PVector();

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND,
				centroDeGravidadeDaMaoEsquerda);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND,
				centroDeGravidadeDaMaoDireita);

		centroDeGravidadeDaMaoDireita = converterPara2D(centroDeGravidadeDaMaoDireita);
		centroDeGravidadeDaMaoEsquerda = converterPara2D(centroDeGravidadeDaMaoEsquerda);

		PVector[] centroDeGravidadeDasMaos = { centroDeGravidadeDaMaoEsquerda,
				centroDeGravidadeDaMaoDireita };

		return centroDeGravidadeDasMaos;
	}

	private void adicionarMomentoDaMao(int userId) {
		PVector[] centroDeGravidadeDaMao = getCentroGravidadeDasMaos(userId);
		somaMomentosX += centroDeGravidadeDaMao[esquerdo].x * 0.007f;
		somaMomentosY += centroDeGravidadeDaMao[esquerdo].y * 0.007f;

		somaMomentosX += centroDeGravidadeDaMao[direito].x * 0.007f;
		somaMomentosY += centroDeGravidadeDaMao[direito].y * 0.007f;
	}

	// ////////////////////////////Punho///////////////////////////////////////
	public PVector[] getPunhos(int userId) {
		PVector cotoveloEsquerdo = new PVector();
		PVector maoEsquerda = new PVector();

		PVector cotoveloDireito = new PVector();
		PVector maoDireita = new PVector();

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND,
				maoEsquerda);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW,
				cotoveloEsquerdo);

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND,
				maoDireita);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW,
				cotoveloDireito);

		cotoveloEsquerdo = converterPara2D(cotoveloEsquerdo);
		maoEsquerda = converterPara2D(maoEsquerda);

		cotoveloDireito = converterPara2D(cotoveloDireito);
		maoDireita = converterPara2D(maoDireita);

		maoEsquerda.mult(0.70f);
		cotoveloEsquerdo.mult(0.30f);

		maoDireita.mult(0.70f);
		cotoveloDireito.mult(0.30f);

		maoEsquerda.add(cotoveloEsquerdo);
		maoDireita.add(cotoveloDireito);

		PVector[] maos = { maoEsquerda, maoDireita };

		return maos;
	}

	// ////////////////////////////////////////////////////////////////////////
	// /////////////////////////////Antebraço//////////////////////////////////
	private PVector calcCentroGravidadeDoAntebraco(PVector cotovelo, PVector punho) {

		cotovelo.mult(0.61f);
		punho.mult(0.39f);
		cotovelo.add(punho);

		return cotovelo;
	}

	public PVector[] getCentroGravidadeDoAntebraco(int userId) {

		PVector cotoveloEsquerdo = new PVector();
		PVector cotoveloDireito = new PVector();

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW,
				cotoveloEsquerdo);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW,
				cotoveloDireito);


		cotoveloEsquerdo = converterPara2D(cotoveloEsquerdo);
		cotoveloDireito = converterPara2D(cotoveloDireito);

		PVector centroDeGravidadeDoAntebracoEsquerdo 
						= calcCentroGravidadeDoAntebraco(cotoveloEsquerdo, getPunhos(userId)[esquerdo]);

		PVector centroDeGravidadeDoAntebracoDireito = calcCentroGravidadeDoAntebraco(
				cotoveloDireito, getPunhos(userId)[direito]);

		PVector[] centrosDeGravidadeDosAntebracos = {
				centroDeGravidadeDoAntebracoEsquerdo,
				centroDeGravidadeDoAntebracoDireito };
		
		return centrosDeGravidadeDosAntebracos;
	}

	private void adicionarMomentoDoAntebraco(int userId) {
		PVector[] centroGravidadeDaPanturrilha = getCentroGravidadeDoAntebraco(userId);

		somaMomentosX += centroGravidadeDaPanturrilha[esquerdo].x * 0.016;
		somaMomentosY += (centroGravidadeDaPanturrilha[esquerdo].y) * 0.016;
		somaMomentosX += centroGravidadeDaPanturrilha[direito].x * 0.016;
		somaMomentosY += (centroGravidadeDaPanturrilha[direito].y) * 0.016;
	}

	// ////////////////////////////////////////////////////////////////////////
	///////////////////////////Pé/////////////////////////////////////////////
	public PVector[] getCentroGravidadeDoPe(int userId) {

		PVector peEsquerdo = new PVector();
		PVector peDireito = new PVector();

		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_FOOT,
				peEsquerdo);
		context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_FOOT,
				peDireito);


		peEsquerdo = converterPara2D(peEsquerdo);
		peDireito = converterPara2D(peDireito);

		PVector[] centrosDeGravidadeDosPes = {
				peEsquerdo,
				peDireito };
		
		return centrosDeGravidadeDosPes;
	}
	
	public void adicionarMomentoDoPe(int userId){
		PVector[] centroDeGravidadePe=getCentroGravidadeDoPe(userId);
		
		somaMomentosX += centroDeGravidadePe[esquerdo].x * 0.015;
		somaMomentosY += (centroDeGravidadePe[esquerdo].y) * 0.015;
		somaMomentosX += centroDeGravidadePe[direito].x * 0.015;
		somaMomentosY += (centroDeGravidadePe[direito].y) * 0.015;
	}

	///////////////////////////////////////////////////////////////////////////
	private float somaMomentosX = 0;
	private float somaMomentosY = 0;

	public PVector getCentroDeGravidade2(int userId) {
		somaMomentosX = 0;
		somaMomentosY = 0;
		adicionarMomentoBracoSuperior(userId);
		adicionarMomentoDoTronco(userId);
		adicionarMomentoDasCoxas(userId);
		adicionarMomentoDaCabeca(userId);
		adicionarMomentoDaPanturrilha(userId);
		adicionarMomentoDaMao(userId);
		adicionarMomentoDoAntebraco(userId);
		adicionarMomentoDoPe(userId);
		return new PVector(somaMomentosX, somaMomentosY);
	}

	public double getDistancia(int userId) {
		double soma = 0;
		double quantidade = 0;

		int pos;

		int pixels[] = context.getUsersPixels(userId);

		for (int y = 0; y < 420; y++) {

			for (int x = 0; x < 640; x++) {

				pos = x + y * 640;
				if (pixels[pos] != 0) {
					soma += context.depthMap()[pos];
					quantidade += 1;
				}
			}

		}
		return (((soma / quantidade / 25.4) * 2.54) / 100);
	}

	public PVector getPontoMaisBaixo(int userId) {
		int pos;

		int pixels[] = context.getUsersPixels(userId);

		PVector pontoMaisAlto = new PVector();

		for (int y = 0; y < 480; y++) {

			for (int x = 0; x < 640; x++) {

				pos = x + y * 640;
				if (pixels[pos] != 0) {
					pontoMaisAlto = context.depthMapRealWorld()[pos];
					break;
				}
			}

		}

		return pontoMaisAlto;
	}

	public PVector getPontoMaisAlto(int userId) {
		int pos;

		int pixels[] = context.getUsersPixels(userId);

		PVector pontoMaisBaixo = new PVector();

		for (int y = context.depthHeight()-1; y >= 0; y--) {
			for (int x = 0; x < 640; x++) {
				pos = x + y * 640;
				if (pixels[pos] != 0) {
					pontoMaisBaixo = context.depthMapRealWorld()[pos];
					break;
				}
			}

		}

		return pontoMaisBaixo;
	}

	public PVector getCentroDeGravidade(int userId) {
		PVector pontoM = new PVector();
		context.getCoM(userId, pontoM);
		return pontoM;
	}

	public PVector converterPara2D(PVector point) {
		PVector point2 = new PVector();
		context.convertRealWorldToProjective(point, point2);
		return point2;
	}

}
